//
// Created by oftenlin on 23-9-13.
//

#ifndef LANGGE_VDR_NAVIFUSIONIMPL_HPP
#define LANGGE_VDR_NAVIFUSIONIMPL_HPP
#include "NaviData.h"
class INaviFusion{
public:
    // 系统初始化
    virtual void Init(NaviData::NavInfo &initPos,uint32_t state_num,uint32_t noise_num) = 0;
    // 预测
    virtual void Predict(std::shared_ptr<NaviData::ImuData>& imuDataPtr) = 0;
    // 修正
    virtual void Correct(std::shared_ptr<NaviData::BaseData>& naviDataPtr) = 0;

    // 对外的位置姿态接口
    virtual void GetPos(NaviData::NavInfo &pos) =0;
    
protected:
    // 更新雅可比矩阵
    virtual void UpdateJacobianMatrix() = 0;
    // 当前的导航信息
    NaviData::NavInfo curNaviInfo_;
    // 当前的导航信息
    NaviData::NavInfo preNaviInfo_;
    // 状态量
    Eigen::VectorXd x_;
    // 预测模型先验误差
    Eigen::MatrixXd xp_;
    // 系统状态和预测模型的误差转换关系
    Eigen::MatrixXd G_;
    // 系统状态量方差
    Eigen::MatrixXd Q_;
    // 状态量的雅可比矩阵
    Eigen::MatrixXd F_;
    // phi = I + F * dt
    Eigen::MatrixXd phi;
    // 系统状态量
    uint32_t state_num_ = 0;
    // 噪声量
    uint32_t noise_num_ = 0;

    bool initialized_ = false;

    bool isIMUUpdate = false;

    bool isStartIns =  false;

    void Measurement(Eigen::VectorXd &z, Eigen::MatrixXd &H_, Eigen::MatrixXd &R_ ,Eigen::VectorXd &x_, Eigen::MatrixXd &xp_);
};
#endif //LANGGE_VDR_NAVIFUSIONIMPL_HPP
